#include <ros/ros.h>
#include <string>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

#include <angles/angles.h>
#include <cmath>
#include <vector>
#include <iostream>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <testlidar/map_builderV3.hpp>
#include <testlidar/tic_toc.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include "opencv2/opencv.hpp"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include "opencv2/opencv.hpp"

#include <cv_bridge/cv_bridge.h>

// #include <testlidar/cpoint_msgs.h>
#include <testlidar/cpoint_msgs.h>
#include <testlidar/rtkGPS.h>
#include <testlidar/rtkGPSmessage.h>

// #define DEG2RAD (57.295779524)

#define DEG2RAD(x) ((x)*0.017453293)
ros::Publisher pubImgClickPoint;

void On_mouse(int event, int x, int y, int flags, void*)
{
    if (flags > 7 && flags < 16 && event == cv::EVENT_LBUTTONDOWN)
    {
//        previousPoint = Point(x, y);
        testlidar::cpoint_msgs msgs;
        msgs.x = x;
        msgs.y = y;
 
        pubImgClickPoint.publish(msgs);
        cout<<x<<","<<y<<endl;
    }
}


    tf::Vector3 tf1;
    double Laititude, Longtitude, YawDeg;
    Eigen::Matrix3d TransferMatrix = Eigen::Matrix3d::Zero();
void GetClickedPointHandler(const geometry_msgs::PointStamped::ConstPtr& msg)
{
    tf1=tf::Vector3(msg->point.x, msg->point.y, msg->point.z);
    Eigen::Vector3d point_2d;
    point_2d(0,0) = msg->point.x/(111000*cos(DEG2RAD(TransferMatrix(1,2))));
    point_2d(1,0) = msg->point.y/111000;
    point_2d(2,0) = 1;
    Eigen::Matrix<double, 3, 1> result = TransferMatrix.cast<double>() * point_2d;

    // broadcaster.sendTransform(
        // tf::StampedTransform(tf::Transform(tf::Quaternion(0.00, 0.00, 0.00, 1), tf::Vector3(msg->point.x, msg->point.y, msg->point.z)),
        // ros::Time::now(),"velodyne", "base_link"));
    cout<<"received a clicked point" << msg->point.x << "," << msg->point.y << "," << msg->point.z<<endl;
    
    cout<<  setprecision(12) << result << endl;

    double LaiError = (result(1,0) - 30.2629191)*111000;
    double LonError = (result(0,0) - 120.1168912)*111000*cos(DEG2RAD(30.2629191));
    double error = sqrt(LaiError*LaiError + LonError*LonError);
    cout<<error<<endl;
    
}
void GetGPSHandler(const receive_gps::rtkGPS::ConstPtr& msg)
{
    TransferMatrix(0,2) = msg->east_meter;
    TransferMatrix(1,2) = msg->north_meter;
    TransferMatrix(2,2) = 1;
    TransferMatrix(0,0) = cos(DEG2RAD(-msg->yaw_rad + 180));
    TransferMatrix(0,1) = -sin(DEG2RAD(-msg->yaw_rad + 180));
    TransferMatrix(1,0) = sin(DEG2RAD(-msg->yaw_rad + 180));
    TransferMatrix(1,1) = cos(DEG2RAD(-msg->yaw_rad + 180));

    // double nm = msg->north_meter;
    Laititude = msg->north_meter;
    Longtitude = msg->east_meter;
    YawDeg = msg->yaw_rad;
//    cout<< setprecision(12) << msg->north_meter << " " <<  msg->east_meter << " "<< setprecision(12) << msg->yaw_rad<<endl;
}
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "testlidar");
    ros::NodeHandle nh;
    ros::Subscriber ClickedPointSub,GetGPSSub;
    uint8_t ImgErrorFLag;

    ClickedPointSub = nh.subscribe<geometry_msgs::PointStamped>
                        ("/clicked_point", 100, GetClickedPointHandler);
    GetGPSSub = nh.subscribe<receive_gps::rtkGPS>
                        ("/rtkGPS_data", 100, GetGPSHandler);

    pubImgClickPoint = nh.advertise<testlidar::cpoint_msgs>
                        ("/img_point", 1);
    ros::Rate rate(100);

    tf::Transform transform;
    tf::TransformBroadcaster broadcaster;

    cv::VideoCapture vcap;
    cv::Mat image;
    const std::string videoStreamAddress = "http://192.168.1.2:8080/?action=stream";
    vcap.open(videoStreamAddress);
    if(!vcap.open(videoStreamAddress)){
        std::cout<<"Error opening video stream"<<endl;
        ImgErrorFLag = 1;
    }
    else
    {
        vcap >> image;
        cv::imshow("img",image);
        cv::waitKey(1);
        cv::setMouseCallback ("img", On_mouse, 0);
    }
    while(ros::ok())
    {
        if(ImgErrorFLag == 0)
        {
            vcap >> image;
            cv::imshow("img",image);
            cv::waitKey(1);
        }

        broadcaster.sendTransform(
        tf::StampedTransform(tf::Transform(tf::Quaternion(0.00, 0.00, 0.00, 1), tf1),
        ros::Time::now(),"velodyne", "base_link"));

        ros::spinOnce();
        rate.sleep();
    }
    return 0; 
}